//
// Created by fazhehy on 2024/3/22.
//

#include "task.h"

char str[10][64];

float mag_angle;
uint8_t set_mag_deviate_angle(uint8_t keyNum)
{
    if (keyNum == UP || keyNum & ENCODER_POS_PREFIX){
        mag_angle += 1.0f;
    }
    else if (keyNum == DOWN || keyNum & ENCODER_NEG_PREFIX){
        mag_angle -= 1.0f;
    }
    else if (keyNum == BACK){
//        flash_write_parameter();
        return BACK;
    }
    st7789_printf(40, 0, "mag_angle:%f", mag_angle);
    return NO_SHOW;
}

uint8_t car[25];

uint8_t show_car1(uint8_t keyNum)
{
    static uint8_t car1[25];
    static uint8_t time = 0;
    if (time == 0){
        time = 1;
        uint8_t index = 0;
        for (int i = 0; i < 25; ++i) {
            if (car[i] == 1){
                car1[index ++] = i;
            }
        }
    }
    for (int i = 0; i < 10; ++i) {
        st7789_printf(0, 40+i*20, "%d", car1[i]);
        st7789_printf(40, 40+i*20, "%d", car1[10+i]);
        if (20+i < 25)
            st7789_printf(60, 40+i*20, "%d", car1[20+i]);
    }
    if (keyNum == BACK){
        time = 0;
        for (int i = 0; i < 24; ++i) {
            car1[i] = 0;
        }
        return BACK;
    }
    return NO_SHOW;
}

uint8_t show_car2(uint8_t keyNum)
{
    static uint8_t car2[25];
    static uint8_t time = 0;
    if (time == 0){
        time = 1;
        uint8_t index = 0;
        for (int i = 0; i < 25; ++i) {
            if (car[i] == 2){
                car2[index ++] = i;
            }
        }
    }
    for (int i = 0; i < 10; ++i) {
        st7789_printf(0, 40+i*20, "%d", car2[i]);
        st7789_printf(40, 40+i*20, "%d", car2[10+i]);
        if (20+i < 25)
            st7789_printf(60, 40+i*20, "%d", car2[20+i]);
    }
    if (keyNum == BACK){
        time = 0;
        for (int i = 0; i < 24; ++i) {
            car2[i] = 0;
        }
        return BACK;
    }
    return NO_SHOW;
}

uint8_t record_point_task(uint8_t keyNum)
{
    static int16_t head_show = 0, tail_show = 10;
    static int8_t num = 1;

    st7789_printf(40, 20, "len:%03d num:%2d", gnss_target_point_len, num);

    if (gnss_flag == 1){
        gnss_data_parse();
        st7789_printf(40, 0, "lat:%.6f lon:%.6f", gnss.latitude, gnss.longitude);
        gnss_flag = 0;
    }

    if (keyNum == UP || keyNum == DOWN){
        if (gnss_record_point(num)){
            st7789_printf(40, 200, "record error");
        }

        if (gnss_target_point_len < num + 1)
            gnss_target_point_len = num + 1;

        if (keyNum == UP)
            car[num] = 1;
        else
            car[num] = 2;
    }
    else if (keyNum & ENCODER_POS_PREFIX || keyNum == 6){
        num ++;
        if (num >= 25)
            num = 0;
    }
    else if (keyNum & ENCODER_NEG_PREFIX || keyNum == 7){
        num --;
        if (num < 0)
            num = 24;
    }
    else if (keyNum == ENTER){
        head_show += 10;
        if (head_show > 40)
            head_show = 0;
        tail_show += 10;
    }
    else if (keyNum == BACK){
        head_show = 0;
        tail_show = 10;
        calculate_center_point();
        return BACK;
    }
    else if (keyNum == 9){

        head_show = 0;
        for (int i = 0; i < 50; ++i) {
            gnss_target_point[i].latitude = 0;
            gnss_target_point[i].longitude = 0;
            if (i < 25)
                car[i] = 0;
        }
        gnss_target_point_len = 1;
    }


    for (int i = 0; i < 10; ++i) {
        st7789_printf(0, 40+20*i, "lat:%.6f lon:%.6f", gnss_target_point[head_show+i].latitude, gnss_target_point[head_show+i].longitude);
    }

    st7789_printf(40, 260, "head:%03d", head_show);

    return NO_SHOW;
}

uint8_t set_lamp_num(uint8_t keyNum)
{
    static int8_t group = 1;
    static int8_t pos = 0, order = 1, isSelect = 0;

    st7789_printf(40, 0, "set lamp num");
    st7789_printf(40, 20, "group:%02d   order:%02d", group+pos, order);
    st7789_printf(40, 260, "groupLen:%02d", groupLen);

    if (keyNum != NOP){
        switch (pos) {
            case 0:
                st7789_draw_rectangle(0, 38, 240, 64, ST7789_BG_COLOR);
                st7789_draw_round_rectangle(24*(order%10), 39+20*(order/10), 22, 20, 5, ST7789_BG_COLOR);
                break;
            case 1:
                st7789_draw_rectangle(0, 108, 240, 64, ST7789_BG_COLOR);
                st7789_draw_round_rectangle(24*(order%10), 109+20*(order/10), 22, 20, 5, ST7789_BG_COLOR);
                break;
            case 2:
                st7789_draw_rectangle(0, 178, 240, 64, ST7789_BG_COLOR);
                st7789_draw_round_rectangle(24*(order%10), 179+20*(order/10), 22, 20, 5, ST7789_BG_COLOR);
                break;
            default:
                break;
        }
    }

    if ((keyNum & ENCODER_POS_PREFIX) || keyNum == UP || keyNum == 6){
        if (isSelect){
            if ((keyNum & ENCODER_POS_PREFIX) || keyNum == 6){
                order --;
                if (order <= 0)
                    order = MAX_SEQ_LEN-1;
            }
            else{
                lampGroup[group+pos].sequence[order] ++;
                if (groupLen < group+pos+1)
                    groupLen = group+pos+1;
            }
        }
        else{
            pos --;
            if (pos < 0){
                group --;
                if (group <= 0)
                    group = 1;
                pos = 0;
            }
        }
    }
    else if ((keyNum & ENCODER_NEG_PREFIX) || keyNum == DOWN || keyNum == 7){
        if (isSelect){
            if ((keyNum & ENCODER_NEG_PREFIX) || keyNum == 7){
                order ++;
                if (order >= MAX_SEQ_LEN)
                    order = 1;
            }
            else{
                lampGroup[group+pos].sequence[order] --;
                if (lampGroup[group+pos].sequence[order] < 0)
                    lampGroup[group+pos].sequence[order] = 0;
                else{
                    if (groupLen < group+pos+1)
                        groupLen = group+pos+1;
                }
            }

        }
        else{
            pos ++;
            if (pos >= 3){
                group ++;
                if (group > MAX_GROUP_LEN)
                    group = MAX_GROUP_LEN-1;
                pos = 2;
            }
        }
    }
    else if (keyNum == ENTER){
        if (isSelect)
            isSelect = 0;
        else{
            isSelect = 1;
            order = 1;
        }
    }
    else if (keyNum == BACK){
        order = 1;
        isSelect = 0;
        return BACK;
    }
    else if (keyNum == 9){
        for (int i = 0; i < MAX_GROUP_LEN; ++i) {
            for (int j = 0; j < MAX_SEQ_LEN; ++j) {
                lampGroup[i].sequence[j] = 0;
            }
        }
        groupLen = 1;
    }

    for (int i = 0; i < 10; ++i) {
        st7789_printf(2+24*i, 40, "%02d", lampGroup[group].sequence[i]);
        st7789_printf(2+24*i, 60, "%02d", lampGroup[group].sequence[10+i]);
        st7789_printf(2+24*i, 80, "%02d", lampGroup[group].sequence[20+i]);

        st7789_printf(2+24*i, 110, "%02d", lampGroup[group+1].sequence[i]);
        st7789_printf(2+24*i, 130, "%02d", lampGroup[group+1].sequence[10+i]);
        st7789_printf(2+24*i, 150, "%02d", lampGroup[group+1].sequence[20+i]);

        st7789_printf(2+24*i, 180, "%02d", lampGroup[group+2].sequence[i]);
        st7789_printf(2+24*i, 200, "%02d", lampGroup[group+2].sequence[10+i]);
        st7789_printf(2+24*i, 220, "%02d", lampGroup[group+2].sequence[20+i]);
    }
    switch (pos) {
        case 0:
            st7789_draw_rectangle(0, 38, 240, 64, ST7789_PEN_COLOR);
            if (isSelect)
                st7789_draw_round_rectangle(24*(order%10), 39+20*(order/10), 22, 20, 5, ST7789_COLOR_BLACK);
            break;
        case 1:
            st7789_draw_rectangle(0, 108, 240, 64, ST7789_PEN_COLOR);
            if (isSelect)
                st7789_draw_round_rectangle(24*(order%10), 109+20*(order/10), 22, 20, 5, ST7789_COLOR_BLACK);
            break;
        case 2:
            st7789_draw_rectangle(0, 178, 240, 64, ST7789_PEN_COLOR);
            if (isSelect)
                st7789_draw_round_rectangle(24*(order%10), 179+20*(order/10), 22, 20, 5, ST7789_COLOR_BLACK);
            break;
        default:
            break;
    }
    return NO_SHOW;
}

uint8_t flash_write_point_task(uint8_t keyNum)
{
    flash_write_gnss_point();
    st7789_printf(0, 40, "flash write OK!");
    system_delay_ms(100);
    return BACK;
}

uint8_t remote_control_task(uint8_t keyNum)
{
    if (!nrf24l01_flag)
        return BACK;
    nrf24l01_enable_tx();
    uint32_t disconnect_times = 0;
    int16_t servo_duty = 0;
    int32_t temp = 0;
//    int16_t duty = 0;
    servo_init();
    while (1){
        if (remote.protocolState == 1){
            remote.protocolState = 0;
            disconnect_times = 0;
        }
        keyNum = remote.operation;
        st7789_printf(40, 20, "%d, %d", remote.adcValue1, remote.medAdcValue1);
        st7789_printf(40, 40, "%d, %d", remote.adcValue2, remote.medAdcValue2);
        st7789_printf(40, 60, "%d", keyNum);
        temp = (int32_t )((int )(remote.adcValue1-remote.medAdcValue1)*(float )800/2040.0f);
        servo_duty = (int16_t )((int )(remote.adcValue2-remote.medAdcValue2)*(float)SERVO_LIMIT/2040.0f);
        if (temp <= 100 && temp >= -100) {
            *p_target_bldc_duty = 0;
        }
        else {
            *p_target_bldc_duty = temp;
        }
        if (servo_duty <= 10 && servo_duty >= -10)
            servo_duty = 0;

        st7789_printf(40, 80, "%4d", *p_target_bldc_duty);
        st7789_printf(40, 100, "%4d", servo_duty);
        st7789_printf(40, 120, "%d", disconnect_times);
        SCB_CleanInvalidateDCache();
        servo_set_duty(SERVO_MEDIAN+servo_duty);
        disconnect_times ++;
        if (disconnect_times >= 100){
            nrf24l01_flag = false;
            *p_target_bldc_duty = 0;
            SCB_CleanInvalidateDCache();
            break;
        }
        if (keyNum == BACK || get_key_num() == BACK)
            break;
    }
    nrf24l01_disable_tx();
    return BACK;
}

uint8_t mag_calibration_task(uint8_t keyNum)
{
    nrf24l01_enable_tx();
    while (1){
        keyNum = get_key_num();
        st7789_printf(0, 40, "mx:%.3f\nmy:%.3f\nmz:%.3f", raw_mx, raw_my, raw_mz);
        st7789_printf(0, 100, "gz:%.3f", raw_gz);
        if (raw_mx != 0 && raw_my != 0 && raw_mz != 0){
            vofa_send_data(raw_mx);
            vofa_send_data(raw_my);
            vofa_send_data(raw_mz);
            vofa_send();
        }
        if (keyNum == BACK)
            break;
    }
    nrf24l01_disable_tx();
    return BACK;
#if 0
    static uint8_t times = 0;
    static float mx, my, mz;
//    float alpha = 0.3f;
    if (times == 0){
//        st7789_clear();
        st7789_show_string(40, 2, "mag calibration");
        times = 1;
    }

    imu963ra_get_mag();
//    mx = imu963ra_mag_transition(imu963ra_mag_x)*alpha+mx*(1-alpha);
//    my = imu963ra_mag_transition(imu963ra_mag_y)*alpha+my*(1-alpha);
//    mz = imu963ra_mag_transition(imu963ra_mag_z)*alpha+mz*(1-alpha);
    mx = imu963ra_mag_transition(imu963ra_mag_x);
    my = imu963ra_mag_transition(imu963ra_mag_y);
    mz = imu963ra_mag_transition(imu963ra_mag_z);

    if (mx != 0 && my != 0 && mz != 0)
        AHRS_mag_calibration(mx, my, mz);

//    nrf24l01_printf("%f,%f,%f\n", mx, my, mz);
    st7789_printf(0, 40, "mx:%.3f\nmy:%.3f\nmz:%.3f", mx, my, mz);
    mx = (mx-mx_offset)/mx_k;
    my = (my-my_offset)/my_k;
    mz = (mz-mz_offset)/mz_k;
    st7789_printf(40, 20, "mag calibration flag:%d", mag_calibration_flag);

    st7789_printf(0, 100, "x_o:%.3f\nx_k:%.3f", mx_offset, mx_k);
    st7789_printf(0, 140, "y_o:%.3f\ny_k:%.3f", my_offset, my_k);
    st7789_printf(0, 180, "z_o:%.3f\nz_k:%.3f", mz_offset, mz_k);
    st7789_printf(100, 40, "mx:%.3f", mx);
    st7789_printf(100, 60, "my:%.3f", my);
    st7789_printf(100, 80, "mz:%.3f", mz);
    if (keyNum == UP || keyNum == DOWN){
        mag_calibration_flag = 0;
    }
    else if (keyNum == ENTER){
        mag_calibration_flag += 1;
        if (mag_calibration_flag > 2)
            mag_calibration_flag = 2;
    }
    else if (keyNum == BACK){
        times = 0;
        if (mag_calibration_flag == 3)
            st7789_printf(0, 120, "calibration OK!");
        system_delay_ms(100);
//        st7789_clear();
        return BACK;
    }
    return NO_SHOW;
#endif
}

uint8_t show_attitude_task(uint8_t keyNum)
{
    static uint8_t times = 0;

    if (times == 0){
        nrf24l01_enable_tx();
        st7789_show_string(40, 2, "show attitude");
        times = 1;
    }

    AHRS_show_angle();

    if (keyNum == BACK){
        nrf24l01_disable_tx();
        times = 0;
        return BACK;
    }

    return NO_SHOW;
}

uint8_t flash_write_lamp_task(uint8_t keyNum)
{
    flash_write_lamp_num();
    st7789_printf(0, 40, "flash write OK!");
    system_delay_ms(100);
    return BACK;
}

uint8_t flash_write_param_task(uint8_t keyNum)
{
    flash_write_parameter();
    st7789_printf(0, 40, "flash write OK!");
    system_delay_ms(100);
    return BACK;
}

uint8_t recover_all_param(uint8_t keyNum)
{
    flash_write_default();
    st7789_printf(0, 40, "recover OK!");
    system_delay_ms(100);
    return BACK;
}

uint8_t max_order = 1;
uint8_t adjust_current(uint8_t keyNum)
{
    static int8_t pos = 0, isSelect = 0;

    if (keyNum != NOP)
        st7789_printf(0, 40+pos*20, "    ");

    if (keyNum == UP || keyNum & ENCODER_POS_PREFIX){
        if (isSelect){
            switch (pos) {
                case 0: *p_max_current += 10;break;
                case 1:
                    if (*p_stop_flag_flag)
                        *p_stop_flag_flag = 0;
                    else
                        *p_stop_flag_flag = 1;
                    break;
                case 2: max_order ++; break;
                default:
                    break;
            }
        }
        else{
            pos --;
            if (pos < 0)
                pos = 2;
        }
    }
    else if (keyNum == DOWN || keyNum & ENCODER_NEG_PREFIX){
        if (isSelect){
            switch (pos) {
                case 0: *p_max_current -= 10;break;
                case 1:
                        if (*p_stop_flag_flag)
                            *p_stop_flag_flag = 0;
                        else
                            *p_stop_flag_flag = 1;
                        break;
                case 2: max_order --;
                        if (max_order == 0)
                            max_order = 1;
                        break;
                default:
                    break;
            }
        }
        else{
            pos ++;
            if (pos == 3)
                pos = 0;
        }
    }
    else if (keyNum == ENTER){
        if (isSelect == 1)
            isSelect = 0;
        else
            isSelect = 1;
    }
    else if (keyNum == BACK){
        SCB_CleanInvalidateDCache();
        pos = 0;
        isSelect = 0;
        return BACK;
    }
    st7789_printf(40, 40, "max current:%f", *p_max_current);
    st7789_printf(40, 60, "current flag:%d", *p_stop_flag_flag);
    st7789_printf(40, 80, "max_order:%02d", max_order);
    if (isSelect == 0)
        st7789_printf(0, 40+pos*20, "-->");
    else
        st7789_printf(0, 40+pos*20, "===");
    return NO_SHOW;
}

uint8_t learn_task(uint8_t keyNum)
{
    lamp_seq_init();
    uint8_t order = 1;
    uint8_t last_num = 0, now_num = 0;
    uint8_t predict_num = 0;
    while (lora_receive_num == 0){
        st7789_printf(40, 0, "ready learn");
    }

    while (1){
        now_num = lora_receive_num;
        if (get_key_num() == BACK)
            break;
        if (now_num == 0)
            break;
        if (last_num != now_num){
            search_match_group(order, now_num);
            predict_num = predict_lamp_num(order);
            order ++;
            last_num = now_num;
        }
        for (int i = 0; i < 10; ++i) {
            st7789_printf(22*i, 100, "%d", lampGroup[0].sequence[i]);
            st7789_printf(22*i, 120, "%d", lampGroup[0].sequence[10+i]);
            st7789_printf(22*i, 140, "%d", lampGroup[0].sequence[20+i]);
        }
        st7789_printf(0, 40, "now:%02d", now_num);
        st7789_printf(0, 60, "predict:%02d", predict_num);
        st7789_printf(0, 80, "next order:%02d", order);
    }
    if (order >= max_order){
        add_new_sequence();
//        beep_toggle(200);
    }
    return BACK;
}

uint8_t test_brake(uint8_t keyNum)
{
    float distance = 10, angle = 0;

    float yaw;
    float target_angle = 0;

    bool update_flag = false;
    bool brake_flag = false;

    int16_t duty = 0;

    servo_init();
    position_pid_init(&servo_pid, outdoor_learn_gnss.p, outdoor_learn_gnss.i, outdoor_learn_gnss.d, SERVO_LIMIT, 300);
    servo_set_duty(SERVO_MEDIAN);

    while (lora_receive_num == 0){
        st7789_printf(40, 0, "Ready!!!!");
    }
    st7789_printf(40, 0, "Start!!!!");

    *p_target_bldc_duty = 1200;
    SCB_CleanInvalidateDCache();

    while (1){
        if (lora_receive_num == 0){
            break;
        }
        

        if (gnss_flag == 1){
            gnss_data_parse();
            distance = (float )get_two_points_distance(gnss.latitude, gnss.longitude,gnss_target_point[0].latitude, gnss_target_point[0].longitude);
            angle = (float )get_two_points_azimuth(gnss.latitude, gnss.longitude,gnss_target_point[0].latitude, gnss_target_point[0].longitude);
            gnss_flag = 0;
            update_flag = true;
        }

        yaw = convert_angle(eulerAngle.yaw);
        target_angle = (float )servo_input_angle(angle,yaw);

        if (brake_flag){
            *p_target_bldc_duty = 0;
            SCB_CleanInvalidateDCache();
            break;
        }

        duty = (int16_t)position_pid(&servo_pid, target_angle, 0);
        servo_set_duty((int16_t)(duty+SERVO_MEDIAN));

        if (update_flag){
            SCB_CleanInvalidateDCache();
            if (distance <= 0.0028*(*p_bldc_speed/1000)*(*p_bldc_speed/1000)){
                brake_flag = true;
            }
            else{
                if (fabsf(target_angle) > 70){
                    *p_target_bldc_duty = (int16_t )(800 - 800*0.5f*fabsf(target_angle)/180);
                }
                else{
                    *p_target_bldc_duty = (int16_t )(outdoor_precise_gnss.speed - outdoor_precise_gnss.speed*0.5f*fabsf(target_angle)/180);
                }
                SCB_CleanInvalidateDCache();
            }
            update_flag = false;
        }
    }

    servo_set_duty(SERVO_MEDIAN);
    motor_stop();
    *p_target_bldc_duty = 0;
    SCB_CleanInvalidateDCache();
    return BACK;
}

//uint8_t test_imu(uint8_t keyNum)
//{
//    nrf24l01_enable_tx();
//    servo_init();
//    position_pid_init(&servo_pid, outdoor_precise_gnss.p, outdoor_precise_gnss.i, outdoor_precise_gnss.d, SERVO_LIMIT, 300);
//    servo_set_duty(SERVO_MEDIAN);
//    *p_target_bldc_duty = 300;
//    SCB_CleanInvalidateDCache();
//    while(1){
//        float yaw = convert_angle(eulerAngle.yaw);
//        float target_angle = servo_input_angle(100,yaw);
//        int16_t duty = (int16_t)position_pid(&servo_pid, target_angle, 0);
//        servo_set_duty(duty+SERVO_MEDIAN);
//        vofa_send_data(yaw);
//        vofa_send();
//        if (remote.protocolState != 0){
//            remote.protocolState = 0;
//            if (remote.operation != 0)
//                break;
//        }
//        if (lora_receive_num != 0){
//            break;
//        }
//    }
//    motor_stop();
//    *p_target_bldc_duty = 0;
//    SCB_CleanInvalidateDCache();
//    nrf24l01_disable_tx();
//    return BACK;
//}
